/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef LASER_ODOMETRY_H
#define LASER_ODOMETRY_H

/////////// ALGORITHIM DEFINITIONS ///////////
// BASIC_ODOMETRY_ONLY:: uses only Nuppu + heading. Optional to VER1&2
// VER1 & VER2:: Laserodometry options - Use only other 
// USE_MCL & USE_MAP_CORRELATION:: A priori localisation algorithms - Use only other  
// USE_CORRIDOR:: Topological locator based on corridor crossings. Can be used with all versions 
//		However the corridor locator depends on the  map, and currently implemented to the MCL
////////////////////////////////////////////////////////////////////

#define ROBOT_VERSION


//#define BASIC_ODOMETRY_ONLY
//#define VER1	//FOR SICKMAP IF !def THEN ANGLE + 2D Correlation
#define VER2
#define USE_MCL //FOR USING MONTE CARLO LOCALISATION
//#define USE_MAP_CORRELATION
//#define USE_CORRIDOR

/**
 * Do we want to use NUPPU/SiLMU and compass+gyro for
 * initial guess or do we rely solely on laser.
 */
//#define NO_INITIAL_GUESS 

#include "scanMatch.h"
#include "kalmanfilter.h"
#include "tpose.h"
#include "linelib.h"
#include "error_handler.h"
#include "tpose.h"
//#include "serialdrv.h"
//#include "sickinit.h"
//#include "sick.h"
#include "sickmapf2LSQ.h"
#include "scanMatch.h"
#include "laserMath.h"
#include "segmentation.h"
#include "laserOdometryInterface.h"
#include "lms200.h"
#include "mapHandling.h"
#include "poseCorrection.h"
#include "timeStamp.h"
#include "corridorLocator.h"

#ifdef USE_MCL
	#include "particleMath.h"
#endif

#ifdef ROBOT_VERSION
	#include ".\J2B2interface\J2B2interface.h"
#endif

/**
 * If defined, laser has no main function and
 * it can be used as module.
 */
#define USE_LASER_AS_MODULE

#define POSEBUFFERSIZE 2000
#define TRACK_POSE_BUFFER_SIZE 2000
#define NUMOFMEASUREMENTS 181.0					// Number of laser measurements used in scan matching
#define HITNUMBER ((250.0/360.0)*NUMOFMEASUREMENTS)	// Goodness value for scan matching

/**
 * Enables debug printing for laser
 */
#define LASER_DEBUG

typedef struct __poseBuffer{
 TPose poses[POSEBUFFERSIZE];
 int numOfPosesInBuffer;
}TPoseBuffer;

///////////////////////////////////////////////////////////////////////
/////////////////// MCL DATA STRUCT ///////////////////////////////////
///////////////////////////////////////////////////////////////////////

#ifdef USE_MCL

#define NUM_OF_PARTICLES 400

//#define MCL_DEBUG

typedef struct __TMCLData{
	TLineVector map;				// Apriori Map
	TPoseParticleDistribution PPD;	// Particle distribution
	float Q[3];						// Uncertainty
	bool isInitialized;				// tells if particle filter is initialized
	float v[3];						// Motion between last position and current position (in absolute coordinates!)
	float *scan;					// Current measurement (x,y) vector
	float Qini[3];					// Uncertainty of initial position
	int numOfSIRupdates;			// How many updates without SIR
	TPose meanPos;					// Average of pose distribution	
	TPose varPos;					// Variance of the pose distribution
	int timesBadLik;
}TMCLData;
#endif

/////////////////////////////////////////////////////////////////////////

// DATA STRUCTURES FOR SCAN MATCHING
/*************************  SCAN DATA ********************
* Structure for representing the scan in polar coordinates 
***********************************************************/
typedef struct __ScanData{
	float *r;		// Laser measurements from latest scan (size=N_ACT_MEAS, max 361)
	float *a;		// Relative Angles for the latest measurements (size=N_ACT_MEAS, max 361)
	int NScanData;  // The number of measurements in latest scan vector
	float Scantime;	// The time stamp for the measurement; 
} TScanData;

/*********************** ScanXYdata ********************************
* Structure for representing the scan (or x,y map) in X,Y coordinates
*********************************************************************/
typedef struct __ScanXYdata{
	float *xRef;	// The X coordinates of the scan  
	float *yRef;	// The Y coordinates of the scan  
	int NScanData;	// Number of measurements in X,Y data
	float Scantime;	// Time stamp for the map measurement
} TScanXYdata;


/********************* Matching data *******************************
* Structure for encapsulating the data needed in the scan matching
********************************************************************/
typedef struct __MatchingData {
	TPose currentPosition;	// Current Position x,y,phii
	TPose refScanPosition;	// Position of the Reference scan
	float *matchPosition;	// (size 3) Relative position between reference scan and current scan

	TScanData actScan;		// Latest scan in Polar coordinates	
	TScanXYdata refScan;	// Reference scan in X,Y coordinates;

	int hitNumber;			/** number of correspondig points between the **
							 ** reference scan and the latest measurement **/
	TPose refScanSilmuPos;
	TPose integratedLaserPos;

} TMatchingData;


#endif